/*Program to Demonstrate the PID(Proportional,Integral,Derivative) Algorithm 
and the simultaneous interfacing of Keypad and LCD with the PK2100 Controller*/


/*This is to make sure the code runs only on PK2100.It throws a fatal error if the
program is run on another controller*/
#if !(BOARD_TYPE==CPLC_BOARD || BOARD_TYPE==L_STAR)
#fatal "This program only runs on a PK21xx/Rugged Giant or a PK22xx/Little Star"
#endif

// disable the virtual drivers for the io's
#define NOUNIVERSAL 1        

main()
{
    int  n,k,p,q,x,count,q1,s;
    unsigned int i;
    float w,e,f,r,z,sum,x1,kp,ki;
    kp=0.5;
    ki=0.5;
    count=0;
	 
//The below function is used to initialize the LCD.
    uplc_init();
    
// loop forever
    while (1)                
    {
        	 
        while ( (k = lc_kxget(0)) < 0) 
// wait for a key press and return the keynumber in the form of integer.  
            runwatch();
                             
        
// beep for 100 ms       
            up_beep(100);    
//Erases the first line on the LCD.    
            lcd_erase_line(1);
            
//Prints the number which is pressed on the keypad to the console
         printf("%d\n", k);

//count is used to hold the number of keypresses.The keypad can provide numbers from 00-99.
//Two keypresses followed by an enter key advances the program control         
         if(count==0)
         {
//if (k==10) resets k=0, 10 is used here as 0.
         if(k==10)
         k=0;
         p=k;
         }
         if(count==1)
         {
         if(k==10)
         k=0;
         q=k;
         
         }
         if(count==2)
        {
// The input is converted into a number between 0-99.The input here is in angles.
//The motor moves from 0-40C.so we are limited to give the input in angle from 0-40.       
        x=10*p+q;

        lcd_printf(0L, "value = %d degrees", x);
          printf("\nInitially set angle : %d degrees\n",x);
          printf("Proportional Constant Kp : %0.2f \n",kp);          
          printf("Integral Constant Ki : %0.2f \n\n",ki);
//up_dacout() takes input from 0-1023 and writes 0-10.23 volts onto the DAC out.
//this conversion if for converting angle(0-40) to volts(0-1023)        
		  x1=(1023.0/40.0)*(float)x;       
     
        }
//count is incremented after every key press
        count++;
         if(count==3)
         {
//if (k==11) checks for the ENTER key press. Then the value is written onto DAC.
         if(k==11)
       lcd_printf(0x01000000, "value set");
        count=0;
        p=0;
        q=0;
//This conversion is for the internal PID logic. 
                x1=x1/100;        
                  
//This clears the LCD between samples		
        lcd_erase_line(1);
        lcd_erase_line(2);
//reset variables                
        n=0;
		sum=0.0;
   	    q=0;
   	    s=1;

//The code for reading initial samples
         r=up_higain(3)-0.2;
         while(q<10){//while3
          
    	   z=up_higain(3);
         if((z-r)>=0.0 && (z-r)<0.2) {//if

         lcd_printf(0L,"value: %0.2f",z);

         lcd_printf(0x01000000L,"%d",s);

         sum=sum+z;
         if(q%2==0)
         r=r-0.2;
         else
         r=r+0.3;
         q++;
         s++;
         
         
         }//if
    	   }//while3
         sum=sum/10.0;
// Proportional error = Kp*e(t) ; e(t) = Setpoint(SP) - Feedback(FB)
         e=(float)x1-sum;
         f=sum;
     

  while(1){ //while2//


         w=up_higain(3);
		 
// Instantaneous error = Read value - Feedback ; this varies randomly
// lcd_printf() prints characters to LCD.   	   
		 lcd_printf(0L,"Error is : %0.2f",(float)x1-w);
//printf() prints characters to the console.
    	   printf("Error is : %0.2f\t",(float)x1-w);
    	    	   
     	   if((w-f)>0.0 && (w-f)<0.2 ){//if1
         if(n>0)
//Temp setpoint is that point where samples are taken and a new temp setpoint is found until error is zero.
{     	   lcd_printf(0x01000000L,"SetPoint%d is achieved",n);
         printf("\n\nSetPoint%d is achieved\n\n",n);
           }
//up_dacout() takes input from 0-1023 and writes 0-10.23 volts onto the DAC out.
           up_dacout((int)(f*100));

          printf("\n\nProportional Error: %0.3f",e);
          printf("\n\nDACout at SetPoint: %d\n",(int)(f*100));
         

//resetting variables at every stage
			sum=0.0;
   	        q=0;

            s=0;
            r=f-0.2;
         while(q<10){//while3
          
    	   z=up_higain(3);
         if((z-r)>=0.0 && (z-r)<0.2) {//if

         lcd_printf(0L,"value: %0.2f",z);

         lcd_printf(0x01000000L,"%d",s);
// (z-f) is the Integral Error which is "setpoint(SP)(i)- sample(i)"
         printf("\nIntegralError%d%d: %0.3f\n",n,s,(z-f));

// Integrating the Integral Errors 
		 sum=sum+(z-f);
         if(q%2==0)
         r=r-0.2;
         else
         r=r+0.3;
         q++;
         s++;
         
         
         }//if
    	   }//while3
//Averaging the Integral Errors over time which is 10us
         sum=sum/10.0;
//Integral Out is Ki*IntegralError
         lcd_printf(0x01000000L,"Integralout%d: %0.2f",n,(0.8)*sum);
         printf("\nIntegralout%d: %0.3f\n",n,(0.8)*sum);
//Proportional Out is Kp*ProportionalError        
		 e=e*kp;

         
//Manipulated Variable(MV) = MV + Integral Out + Proportional Out	      
     	   f=f+ki*sum+e;
     	   n++;
			}//if1
//MV is fedback and is checked with setpoint(SP) at every stage. e=0 if MV=SP
    	   if(((float)x1-w) < 0.125 ){ //if2
         lcd_printf(0x01000000L,"Error is minimized");
         printf("\n\nError is minimized\n");
//Final setpoint is achieved.
         printf("\n\Final Setpoint is achieved\n");
          
		 up_dacout((int)(w*100));
          printf("\n\nFinal DACout : %d",(int)(w*100));
//Set angle is achieved with +/- 1 degree precision
          printf("\n\n Angle Moved: %0.3f degrees\n\n",(w*4));               
    	   break;
	  	      }//if2
  }//while2//       
   
   	   

    	    
    	    }

         }
         
         
         
        

}